#include "core.h"

  .equ IRQ32_mode, 0x12 /* label to set cpsr in irq mode */
  .equ SVC32_mode, 0x13 /* label to set cpsr in svc mode */
  .equ SYS32_mode, 0x1f /* label to set cpsr in irq mode */
  .equ NO_IRQ,   0xC0 /* disable irq and fiq for safety reasons */
  
  IRQ_STACK_START : .word  0x22f00000/*0x22200000  set irq sp */
  SYS32_STACK_START: .word 0x22f10000/*0x22300000 set system sp*/
  SVC32_STACK_START: .word 0x22f20000/*0x22400000  set svc sp */

/* -------------------------------------------------------------------------- */
/* void init_stacks()                                                         */
/* -------------------------------------------------------------------------- */
.align
.global init_stacks

init_stacks:
  MSR cpsr_c, #(NO_IRQ | SVC32_mode) /* set cpsr to svc mode */
  LDR     sp, SVC32_STACK_START      /* set sp in svc mode */
  MSR CPSR_c,#(NO_IRQ | SYS32_mode)  /* set cpsr to sytem mode */
  LDR     sp, SYS32_STACK_START      /* set sp in system mode */
  MSR CPSR_c,#(NO_IRQ | IRQ32_mode)  /* set cpsr irq mode */
  LDR     sp, IRQ_STACK_START        /* set sp in irq mode */
  MSR CPSR_c,#(NO_IRQ|SVC32_mode)    /* set cpsr to svc mode */
  mov pc, lr                         /* return */
/* -------------------------------------------------------------------------- */


/* -------------------------------------------------------------------------- */
/* void enable_irq_in_core()                                                  */
/* -------------------------------------------------------------------------- */
.align
.global enable_irq_in_core
 
enable_irq_in_core:
  stmfd sp!, {r1}     /* push r1 to stack */
  mrs   r1, cpsr      /* copy cpsr to r1 */
  bic   r1, r1, #0x80 /* clear interrupt bit */
  msr   cpsr_c, r1    /* move r1 to cpsr */
  ldmfd sp!, {r1}     /* pop r1 from stack */
  mov   pc, lr        /* return */
/* -------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------- */
/* void disable_irq_in_core()                                                  */
/* -------------------------------------------------------------------------- */
.align
.global disable_irq_in_core
 
disable_irq_in_core:
  stmfd sp!, {r1}     /* push r1 to stack */
  mrs   r1, cpsr      /* copy cpsr to r1 */
  orr   r1, r1, #0x80 /* set interrupt bit */
  msr   cpsr_c, r1    /* move r1 to cpsr */
  ldmfd sp!, {r1}     /* pop r1 from stack */
  mov   pc, lr        /* return */
/* -------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------- */
/* void yield ()							      */
/* -------------------------------------------------------------------------- */
.align
.global yield

yield:
 STMFD SP!, {lr}
 SWI   
 LDMFD SP!, {pc}

/* -------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------- */
.align
.global handle_sys_interrupt

handle_sys_interrupt:
  STMFD SP!, {r0, r1, r2, r14} /*SWITCH_IRQ_TO_SVC32_mod start*/
  MOV   r1, SP 
  MRS   r2, spsr 
  ADD   SP, #4*4  
  MRS r0, cpsr             
  AND r0, #0xffffffe0 
  ORR r0, #SVC32_mode
  MSR cpsr, r0 
  MSR spsr, r2 
  LDMFD r1, {r0, r1, r2, r14} /*SWITCH_IRQ_TO_SVC32_mod end*/
  sub   lr, lr, #4            /* adjust link register */
  stmfd sp!, {r0-r12, lr}     /* push all registers to stack */
  bl    service_sys_interrupt /* branch to interrupt service routine */
  ldmfd sp!, {r0-r12, pc}^    /* pop all registers from stack and return */
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* void handle_sys_interrupt()                                                */
/* -------------------------------------------------------------------------- */
.align
.global handle_swi_interrupt

handle_swi_interrupt:
  stmfd sp!, {r0-r12, lr}     /* push all registers to stack */
  bl    do_context_switch /* branch to context switch routine */
  ldmfd sp!, {r0-r12, pc}^    /* pop all registers from stack and return */
/* -------------------------------------------------------------------------- */
